/***************************************************************************
 创建者: 华磊
 开始时间: 2019.11.15
 copyright: (C) 华友高科
 ***************************************************************************/
#include "gantrycontrol.h"
#include "motionmessage.h"
#include "axisgroup.h"
#include "filteringalgorithm.h"

GantryControl::GantryControl(QString configFileIn, MotionMessage *motionMessageIn, int robotIdIn, AxisGroup *axisGroupIn)
{
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
    axisGroup=axisGroupIn;
    isInitialOkStatus=false;
    isUseGantryControl=false;

//    filtering_mapFollowAxisPos=new FilteringAlgorithm;

    if(1==initialConfigFile(configFileIn))
    {
        isInitialOkStatus=true;

    }
    else
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "GantryControl", 22300, robotId);
    }
}

bool GantryControl::isGantryControlFollowAxisWork(int followAxisId)
{
//    usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(followAxisId==coupleParameterList[i].followAxisId)
        {
            if(coupleParameterList[i].isValid)
            {
//                usingMutex.unlock();
                return true;
            }
        }
    }
//    usingMutex.unlock();
    return false;
}

bool GantryControl::isInitialOk()
{
    return isInitialOkStatus;
}

bool GantryControl::isUseThisModule()
{
        return isUseGantryControl;
}

int GantryControl::loopRealtimeOnce()
{
    if(false==isUseGantryControl)
    {
        return 0;
    }

//    usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(coupleParameterList[i].isValid)
        {
            calculateOneCouple(coupleParameterList[i],axisGroup);
        }

    }
//    usingMutex.unlock();
    return 1;
}

int GantryControl::setGantryControlEnable(int coupleId, bool isEnableIn)
{
//    usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(coupleId==coupleParameterList[i].followAxisId)
        {
            coupleParameterList[i].isValid=isEnableIn;
//            usingMutex.unlock();
            return 1;
        }

    }
//    usingMutex.unlock();
    return -1;
}

int GantryControl::setGantryControlOffset(int coupleId, double offfsetValueIn)
{
    //    for(int i=0;i<coupleParameterList.size();i++)
    //    {
    //        if(coupleId==coupleParameterList[i].followAxisId)
    //        {
    //            coupleParameterList[i].followOffsetValue=offfsetValueIn;
    //            return 1;
    //        }

    //    }
        return -1;
}

int GantryControl::resetAll()
{
    return 1;
}

int GantryControl::calculateOneCouple(const GantryCtrConfigParameter &couplePara, AxisGroup *axisGroupIn)
{
    QVector<double> jointsPositionOut;
    axisGroupIn->getJointCommand(jointsPositionOut);
    if(jointsPositionOut.size()<=couplePara.driveAxisId || 0>couplePara.driveAxisId)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "GantryControl", 22301, robotId,couplePara.driveAxisId);
        return -1;
    }
    if(jointsPositionOut.size()<=couplePara.followAxisId || 0>couplePara.followAxisId)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "GantryControl", 22302, robotId,couplePara.followAxisId);
        return -1;
    }
    double sourceValue=jointsPositionOut[couplePara.driveAxisId];
    double followValue=jointsPositionOut[couplePara.followAxisId];
    followValue=sourceValue+couplePara.followOffsetValue;
    axisGroupIn->moveOneStepByPosition(followValue,couplePara.followAxisId);

    //根据反馈偏差进行跟随轴的速度补偿
    QVector<double> jointsPositionFeedback, jointsVelFeedback,jointsAccFeedback, jointsTorqueFeedback ;
    axisGroupIn->getJointFeedback(jointsPositionFeedback, jointsVelFeedback,jointsAccFeedback, jointsTorqueFeedback);
    double driveAxisFollowOffset,folowAxisFollowOffset;
    driveAxisFollowOffset=jointsPositionOut[couplePara.driveAxisId]-jointsPositionFeedback[couplePara.driveAxisId];
    folowAxisFollowOffset=followValue-jointsPositionFeedback[couplePara.followAxisId];//no jointsPositionOut
    double followOffsetBias=folowAxisFollowOffset-driveAxisFollowOffset;
    if(fabs(followOffsetBias)>couplePara.allowMaxFollowOffset)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "GantryControl", 22304, robotId,couplePara.followAxisId,
               followOffsetBias,couplePara.allowMaxFollowOffset);
        return -1;
    }
    double velCompensation=followOffsetBias*couplePara.kp_errorControl;
    if(velCompensation>couplePara.maxConpensation)
    {
        velCompensation=couplePara.maxConpensation;
    }
    else if(velCompensation<-couplePara.maxConpensation)
    {
        velCompensation=-couplePara.maxConpensation;
    }
    axisGroupIn->setVelCompensation(velCompensation,couplePara.followAxisId);
    return 1;
}

#define D_PARAMETER_GANTRY_NODE_COUNT 7
int GantryControl::initialConfigFile(QString configFileName)
{
    qDebug() << "GantryControl::initialConfigFile ==== 初始化配置文件：" << configFileName;

    coupleParameterList.clear();


    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << " 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseGantryControl", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isUseGantryControl);

    ///beltParameter读参数
    QDomNode  inNode = domparser.findSubNode(domparser.getRootDomElement(), "coupleParameter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: "<<configFileName;
        }
        return -1;
    }

    int count;

   domparser. readXml( domparser.findSubNode(inNode, "count", ok), count  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: ";
        }
        return -1;
    }


    QVector<bool> okReturn;
    okReturn.resize(D_PARAMETER_GANTRY_NODE_COUNT);
    for(int i=0;i<count;i++)
    {
         QDomNode  tmpNode =  domparser.findSubNode(inNode, QString("couple%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"读内部参数失败:  "<<i;
            }
            return -1;
         }
        GantryCtrConfigParameter tmpConfig;
        //节点读取
        bool tmpValid;
        domparser.readXml( domparser.findSubNode(tmpNode, "isValid", okReturn[0]),  tmpValid );
        tmpConfig.isValid =tmpValid;
        domparser.readXml( domparser.findSubNode(tmpNode, "kp_errorControl", okReturn[1]), tmpConfig.kp_errorControl );
        domparser.readXml( domparser.findSubNode(tmpNode, "driveAxisId", okReturn[2]),  tmpConfig.driveAxisId  );
        domparser.readXml( domparser.findSubNode(tmpNode, "followAxisId", okReturn[3]),  tmpConfig.followAxisId  );
        domparser.readXml( domparser.findSubNode(tmpNode, "allowMaxFollowOffset", okReturn[4]),  tmpConfig.allowMaxFollowOffset  );
        domparser.readXml( domparser.findSubNode(tmpNode, "maxConpensation", okReturn[5]),  tmpConfig.maxConpensation  );
        double tmpFollow;
        domparser.readXml( domparser.findSubNode(tmpNode, "followOffsetValue", okReturn[D_PARAMETER_GANTRY_NODE_COUNT-1]),
                tmpFollow);
        tmpConfig.followOffsetValue =tmpFollow;
//        tmpConfig.isValid=false;
        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"GantryControl::readFile()====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              return -1;
           }
        }
        coupleParameterList.append(tmpConfig);
    }



    domparser.closeXml();
    return 1;
}

void GantryControl::addMsg(int messageLevel, std::string componentName, std::string messageType, int messageCode, int robotIdIn, int parameter1, int parameter2,
                           double parameter3, double parameter4, std::string messageInfo)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22304:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",龙门跟随轴(%1)跟随失败，同步误差(%2)超过允许值(%3)").arg(parameter1)
                .arg(parameter2).arg(parameter3)+QString::fromStdString(messageInfo);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22303:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合跟随轴(%1)超过有效范围").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22302:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合驱动轴(%1)超过有效范围").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22301:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合序号(%1)不存在，设置耦合偏差失败").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22300:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",龙门同步配置文件读取失败");
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"CoupleControlManagement::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
